#include "ToolMover.h"
#include "../QGraphicsView/QGraphicsView.h"
#include <QGraphicsScene>
#include <QLineF>
#include <cmath>
#include <iostream>
#include "../MainWindow/MainWindow.h"
#include "../main.h"
ToolMover::ToolMover(CADView *cadView, QObject *parent)
        : QObject(parent),
        cadView(cadView),
        lineMoveTimer(new QTimer(this)),
        arcMoveTimer(new QTimer(this)),
//一些线宽和颜色设置

          ToolColor(Qt::yellow),//刀具颜色
          CenterPathColor(Qt::red),  // 默认颜色为红色
          LeftEdgePathColor(Qt::yellow),
          RightEdgePathColor(Qt::blue),

          CenterPathWidth(2),        // 默认边框宽度为2
            LeftEdgePathWidth(2),
            RightEdgePathWidth(2)

//          CenterPathFilled(false),    // 默认不填充
//            LeftEdgePathFilled(false),
//            RightEdgePathFilled(false)

{
    // 直线运动定时器
    connect(lineMoveTimer, &QTimer::timeout, this, &ToolMover::updateToolPosition_Line);

    // 圆弧运动定时器
    connect(arcMoveTimer, &QTimer::timeout, this, &ToolMover::updateToolPosition_Arc);
    QPointF toolDefaultPosition(0.0, 0.0);//设置默认刀具位置


    // 初始化路径线和边缘路径线
//直线
    pathLine = new QGraphicsLineItem();
    LiftedgePathLine = new QGraphicsLineItem(); // 左边缘轨迹
    RightedgePathLine = new QGraphicsLineItem(); // 右边缘轨迹

    cadView->scene()->addItem(pathLine);
    cadView->scene()->addItem(LiftedgePathLine);
    cadView->scene()->addItem(RightedgePathLine);

//圆弧
    pathArc = new QGraphicsPathItem();
    LiftedgePathArc = new QGraphicsPathItem();
    RightedgePathArc=new QGraphicsPathItem();


    cadView->scene()->addItem(pathArc);
    cadView->scene()->addItem(LiftedgePathArc);
    cadView->scene()->addItem(RightedgePathArc);
}

ToolMover::~ToolMover()
{
    if (lineMoveTimer->isActive())
    {
        lineMoveTimer->stop();
    }
    deleteTool(); // 确保删除刀具
//     deleteCenterpath();
//     deleteLiftpath();
//     deleteRightpath();
}
//输入参数的意义，开始点，终止点，移动速度，刀具半径，是否展示刀轨
void ToolMover::moveTool_line(const QPointF &start, const QPointF &end, double speed, double R, bool showPath, bool showLiftedgePath, bool showRightedgePath)
{
    //std::cout<<"start:"<<start.x()<<","<<start.y()<<"end:"<<end.x()<<","<<end.y()<<"speed:"<<speed<<"R:"<<R<<std::endl;
    this->showPath = showPath; // 保存路径显示状态
    this->showLiftedgePath = showLiftedgePath; // 保存边缘路径显示状态
    this->showRightedgePath = showRightedgePath; //
    this->R = R * mmToPixel;

    startPosition = start * mmToPixel; // 保存起始位置
    targetPosition = end * mmToPixel;

    // 计算方向向量
    direction = targetPosition - startPosition;

    qreal distance = std::sqrt(direction.x() * direction.x() + direction.y() * direction.y());

    // 将速度从 mm/s 转换为像素/帧
    toolSpeed = speed * mmToPixel * 0.001; // 每帧时间约为 16 ms

    if (distance > 0) {
        direction /= distance; // 归一化方向向量
        pixelSpeed = direction * toolSpeed; // 根据速度设置移动速度（每帧移动的距离）

        // 如果当前刀具为空，则创建一个新的刀具并开始移动
        if (!toolCircle)
        {
            createTool(startPosition, this->R); // 创建刀具并设置初始位置
        } else {
            toolCircle->setPos(startPosition); // 如果刀具已存在，则设置初始位置
        }

        // 如果需要显示路径，初始化路径线
        if (showPath) {
            pathLine->setLine(startPosition.x(), startPosition.y(), startPosition.x(), startPosition.y()); // 初始化路径线
            pathLine->setPen(QPen(CenterPathColor, CenterPathWidth)); // 设置路径线颜色
            cadView->scene()->addItem(pathLine);

        }

        // 如果需要显示边缘路径
        if (showLiftedgePath) {
            LiftedgePathLine->setLine(startPosition.x() + leftcenterEdgeOffset.x(),
                                      startPosition.y() + leftcenterEdgeOffset.y(),
                                      startPosition.x() + leftcenterEdgeOffset.x(),
                                      startPosition.y() + leftcenterEdgeOffset.y()); // 初始化左边缘路径线
            LiftedgePathLine->setPen(QPen(LeftEdgePathColor, LeftEdgePathWidth)); // 设置左边缘轨迹颜色
            cadView->scene()->addItem(LiftedgePathLine); // 将左边缘轨迹添加到场景

        }

        if (showRightedgePath) {
            RightedgePathLine->setLine(startPosition.x() + RightcenterEdgeOffset.x(),
                                       startPosition.y() + RightcenterEdgeOffset.y(),
                                       startPosition.x() + RightcenterEdgeOffset.x(),
                                       startPosition.y() + RightcenterEdgeOffset.y()); // 初始化右边缘路径线
            RightedgePathLine->setPen(QPen(RightEdgePathColor, RightEdgePathWidth)); // 设置右边缘轨迹颜色
            cadView->scene()->addItem(RightedgePathLine); // 将右边缘轨迹添加到场景

        }

        lineMoveTimer->start(1); // 每16毫秒更新一次 (大约60帧每秒)
    }
}

void ToolMover::updateToolPosition_Line()
{
    if (toolCircle)
    {

        toolCircle->moveBy(pixelSpeed.x(), pixelSpeed.y());
        // 计算中心点坐标
        QPointF newEndPoint = toolCircle->mapToScene(toolCircle->rect().center());
        cadView->scene()->update();
        mainWindow->updateXPosition(newEndPoint.x()/mmToPixel);
        mainWindow->updateYPosition(newEndPoint.y()/mmToPixel);

        // 更新刀具的中心点
        if (showPath && pathLine)
        {
            pathLine->setLine(startPosition.x(), startPosition.y(), newEndPoint.x(), newEndPoint.y()); // 更新最终路径线
        }
        calculateToolPathDifference();
        // 更新边缘路径线
        if (showLiftedgePath && LiftedgePathLine) {
            LiftedgePathLine->setLine(startPosition.x() + leftcenterEdgeOffset .x(),
                                      startPosition.y() + leftcenterEdgeOffset .y(),
                                      newEndPoint.x() + leftcenterEdgeOffset .x(),
                                      newEndPoint.y() + leftcenterEdgeOffset .y()); // 更新左边缘路径线
        }

        if (showRightedgePath && RightedgePathLine) {
            RightedgePathLine->setLine(startPosition.x() +RightcenterEdgeOffset.x(),
                                       startPosition.y() + RightcenterEdgeOffset.y(),
                                       newEndPoint.x() + RightcenterEdgeOffset.x(),
                                       newEndPoint.y() + RightcenterEdgeOffset.y()); // 更新右边缘路径线
        }

        // 检查是否到达目标位置
        if (QLineF(newEndPoint, targetPosition).length() <1) {
            toolCircle->setPos(targetPosition); // 确保到达目标位置
            lineMoveTimer->stop(); // 停止定时器
            mainWindow->EXTS(); // 发出信号

            // 最终更新路径线终点
            if (showPath && pathLine) {
                pathLine->setLine(startPosition.x(), startPosition.y(), targetPosition.x(), targetPosition.y()); // 更新最终路径线
            }
        }
    }
}

void ToolMover::createTool(const QPointF &center, double radius)
{
    // 创建图形时，使用相对于 (0, 0) 的位置，以便后续能通过 setPos 精确控制中心点
    toolCircle = new QGraphicsEllipseItem(-radius, -radius, 2 * radius, 2 * radius);

    toolCircle->setBrush(QBrush(ToolColor)); // 填充颜色
//    toolCircle->setPen(QPen(Color, Width));
    cadView->scene()->addItem(toolCircle);

    // 将刀具的中心点设置为指定的中心点
    toolCircle->setPos(center);
}


void ToolMover::deleteTool() //删除刀具
{
    if (toolCircle)
    {
        cadView->scene()->removeItem(toolCircle); // 从场景中移除
        delete toolCircle; // 删除对象
        toolCircle = nullptr; // 重置为 nullptr
    }
}
void ToolMover::deletePath(QGraphicsPathItem*& pathArc)
{
    if (pathArc)
    {
        cadView->scene()->removeItem(pathArc); // 从场景中移除
        delete pathArc; // 删除对象
        pathArc = nullptr; // 重置为 nullptr
    }
}
void ToolMover::deleteLine(QGraphicsLineItem*& line)
{
    if (line)
    {
        cadView->scene()->removeItem(line); // 从场景中移除
        delete line; // 删除对象
        line = nullptr; // 重置为 nullptr
    }
}

void ToolMover::deleteCenterpath()
{
    deleteLine(pathLine);   // 删除中心直线
    deletePath(pathArc);    // 删除中心圆弧路径
}
void ToolMover::deleteLiftpath()
{
    deleteLine(LiftedgePathLine);   // 删除左边直线
    deletePath(LiftedgePathArc);    // 删除左边圆弧路径
}
void ToolMover::deleteRightpath()
{
    deleteLine(RightedgePathLine);  // 删除右边直线
    deletePath(RightedgePathArc);   // 删除右边圆弧路径
}

//输入参数起始点，终止点，圆心，刀具半径，是否展示刀具路径
void ToolMover::moveTool_arc(const QPointF &start, const QPointF &end, const QPointF &Center, double speed, double R, bool showPath, bool showLiftedgePath, bool showRightedgePath)
{
    this->showPath = showPath;
    this->showLiftedgePath = showLiftedgePath;
    this->showRightedgePath = showRightedgePath;

    this->R = R * mmToPixel;

    this->startPosition = start * mmToPixel; // 圆弧起点
    this->targetPosition = end * mmToPixel;  // 圆弧终点
    this->centerPosition = Center * mmToPixel;  // 圆心位置

    this->startAngle=calculateAngle(startPosition ,centerPosition);//起始角度值
    this->endAngle=calculateAngle(targetPosition,centerPosition);//终止角度值

    currentAngle=startAngle;




    std::cout<<"起始角度："<<startAngle<<","<<"终止角度"<<endAngle/3.14*180<<std::endl;
    // 判断是否顺时针或逆时针移动（通过速度）

    if(speed < 0)
    {
        clockwise=false;
        speed=-speed;
    }
    else
    {
        clockwise=true;
    }

    // 计算起点到圆心的向量
    QPointF startVec = startPosition - centerPosition;
    QPointF endVec = targetPosition - centerPosition;

    // 计算圆弧的半径
    double R1;
    double R2;

     R1 = std::sqrt(startVec.x() * startVec.x() + startVec.y() * startVec.y());
     R2=std::sqrt(endVec.x() * endVec.x() + endVec.y() * endVec.y());
     if(R1==R2)
     {
         arcRadius=R1;
     }
     else
     {
         arcRadius=0;
         startAngle=endAngle=0;
     }
//根据刀方向，来设置左右刀轨的半斤
    if(clockwise)
    {
        ArcleftcenterEdgRadius=arcRadius+this->R;//左侧轨迹半径
         ArcRightcenterEdgeRadius=arcRadius-this->R;//右侧轨迹半径


    }
    else
    {
         ArcleftcenterEdgRadius=arcRadius-this->R;//左侧轨迹半径
         ArcRightcenterEdgeRadius=arcRadius+this->R;//右侧轨迹半径

    }
    std::cout<<arcRadius<<std::endl;

    // 将速度从 mm/s 转换为角速度（弧度/秒）
    if(arcRadius==0)
    {
        angularSpeed=0;

    }
    else
    {
        angularSpeed = speed * mmToPixel/ arcRadius; // 角速度：线速度除以半径

    }
   toolSpeed = angularSpeed * 0.016; // 每帧的角度变化量 (帧率为 60fps)

    // 如果当前刀具为空，创建一个新的刀具并开始移动
    if (!toolCircle)
    {
        createTool(startPosition, this->R); // 创建刀具并设置初始位置
    } else {
        toolCircle->setPos(startPosition); // 如果刀具已存在，则设置初始位置
    }

    if (showPath)
    {
        if (!pathArcPath) {
            pathArcPath = new QPainterPath(); // 初始化路径对象
        }
        QPen pen(CenterPathColor);   // 设置边框颜色为红色
        pen.setWidth( CenterPathWidth);      // 设置边框宽度
        pathArc->setPen(pen);
        // 可选：设置圆弧的填充颜色
//        QBrush brush(Qt::NoBrush); // 没有填充
//        pathArc->setBrush(brush);

        pathArcPath->moveTo(startPosition);


    }

   // 初始化边缘路径线
    if (showLiftedgePath)
    {
        if (!LiftedgePathArcPath)
        {
            LiftedgePathArcPath = new QPainterPath(); // 初始化路径对象
        }
        QPen pen(LeftEdgePathColor);   // 设置边框颜色为红色
        pen.setWidth(LeftEdgePathWidth);      // 设置边框宽度
        LiftedgePathArc->setPen(pen);

        LiftedgePathArcPath->moveTo(startPosition);
    }

    if (showRightedgePath) {
        if (!RightedgePathArcPath) {
            RightedgePathArcPath = new QPainterPath(); // 初始化路径对象
        }
        QPen pen(RightEdgePathColor);   // 设置边框颜色为红色
        pen.setWidth(RightEdgePathWidth);      // 设置边框宽度
        RightedgePathArc->setPen(pen);

        RightedgePathArcPath->moveTo(startPosition);
    }
    // 启动定时器
    arcMoveTimer->start(16); // 每 16 毫秒更新一次
}

void ToolMover::updateToolPosition_Arc()
{
    if (toolCircle)
    {
        //更新刀具位置
        double angleChange;
        // 根据顺时针或逆时针调整角度
        if (clockwise)
        {
            angleChange = toolSpeed;  // 顺时针，角度减小
        }
        else
        {
            angleChange = -toolSpeed;    // 逆时针，角度增大
        }


        currentAngle += angleChange;   // 更新当前角度
        std::cout<<"当前角度："<<currentAngle/3.14*180<<std::endl;


        // 计算新位置
        QPointF newPos(centerPosition.x() + arcRadius * std::cos(currentAngle),
                       centerPosition.y() + arcRadius * std::sin(currentAngle));
        std::cout<<centerPosition.x() + arcRadius * std::cos(currentAngle) <<","<<centerPosition.y() + arcRadius * std::sin(currentAngle)<<std::endl;

        // 更新刀具位置
        toolCircle->setPos(newPos);
        cadView->scene()->update();
        // 更新路径线
        mainWindow->updateXPosition(newPos.x());
        mainWindow->updateYPosition(newPos.y());
        if (pathArcPath && pathArc)
        {


            pathArcPath->lineTo(newPos); // 新位置加入到路径
            pathArc->setPath(*pathArcPath); // 更新路径到场景中

        }
        QPointF LiftnewPos(centerPosition.x() + ArcleftcenterEdgRadius * std::cos(currentAngle),
                       centerPosition.y() + ArcleftcenterEdgRadius * std::sin(currentAngle));
        QPointF RightnewPos(centerPosition.x() + ArcRightcenterEdgeRadius * std::cos(currentAngle),
                           centerPosition.y() + ArcRightcenterEdgeRadius * std::sin(currentAngle));

        // 更新边缘路径线
        if (showLiftedgePath && LiftedgePathLine)
        {
            LiftedgePathArcPath->lineTo(LiftnewPos); // 新位置加入到路径
            LiftedgePathArc->setPath(*LiftedgePathArcPath); // 更新路径到场景中
        }

        if (showRightedgePath && RightedgePathLine)
        {
            RightedgePathArcPath->lineTo(RightnewPos); // 新位置加入到路径
            RightedgePathArc->setPath(*RightedgePathArcPath); // 更新路径到场景中
        }

// 检查是否到达终点
        bool reachedEnd = false;

       if(startAngle<endAngle)
       {
           if(clockwise)
           {
               if(currentAngle>=endAngle)
               {
                   reachedEnd=true;
               }
           }
           else if(!clockwise)
           {
               if(currentAngle<=endAngle-2*M_PI)
               {
                   reachedEnd=true;
               }
           }
       }
       else if(startAngle>endAngle)
       {
           if(clockwise)
           {
               if(currentAngle>=endAngle+2*M_PI)
               {
                   reachedEnd=true;
               }
           }
           else if(!clockwise)
           {
               if(currentAngle<=endAngle)
               {
                   reachedEnd=true;
               }
           }
       }
        if (reachedEnd)
        {
            toolCircle->setPos(targetPosition); // 确保到达终点
            arcMoveTimer->stop(); // 停止定时器
            mainWindow->EXTS();  // 发出信号
        }

    }
}

// 计算直线刀心轨迹和刀边轨迹的差值
void ToolMover::calculateToolPathDifference()
{
    // 计算直线的斜率 Line_K
    if ((direction.x() != 0)&&(direction.y() != 0)) {
        Line_K = direction.y() / direction.x();
        // 计算直线与X轴的夹角 angleInRadians
        angleInRadians = std::atan2(direction.y(), direction.x());

        // 使用 angleInRadians 计算中心偏移值

        centerEdgeOffset.setX(std::abs(R * sin(angleInRadians)));
       centerEdgeOffset.setY(std::abs(R * cos(angleInRadians)));
    }
    else if(direction.x() == 0)
    {
        centerEdgeOffset.setX(std::abs(R));
        centerEdgeOffset.setY(std::abs(0));

    }
    else if(direction.y() == 0)
    {
        centerEdgeOffset.setX(std::abs(0));
        centerEdgeOffset.setY(std::abs(R));
    }



    // 根据刀具直线运动方向计算刀具边缘偏移
    if (direction.x() > 0) // 向右运动
    {
        if (direction.y() > 0) // 第一象限//右下
        {
            leftcenterEdgeOffset = QPointF(centerEdgeOffset.x(), -centerEdgeOffset.y());  // 左边缘偏移
            RightcenterEdgeOffset = QPointF(-centerEdgeOffset.x(), centerEdgeOffset.y()); // 右边缘偏移
        }
        else if (direction.y() < 0) // 第四象限
        {
            leftcenterEdgeOffset = QPointF(-centerEdgeOffset.x(), -centerEdgeOffset.y());
            RightcenterEdgeOffset = QPointF(centerEdgeOffset.x(), centerEdgeOffset.y());
        }
        else // direction.y() == 0，水平运动
        {
            leftcenterEdgeOffset = QPointF(0, -centerEdgeOffset.y());
            RightcenterEdgeOffset = QPointF(0, centerEdgeOffset.y());
        }
    }
    else if (direction.x() < 0) // 向左运动
    {
        if (direction.y() > 0) // 第二象限
        {
            leftcenterEdgeOffset = QPointF(centerEdgeOffset.x(), centerEdgeOffset.y());
            RightcenterEdgeOffset = QPointF(-centerEdgeOffset.x(), -centerEdgeOffset.y());
        }
        else if (direction.y() < 0) // 第三象限
        {
            leftcenterEdgeOffset = QPointF(-centerEdgeOffset.x(), centerEdgeOffset.y());
            RightcenterEdgeOffset = QPointF(centerEdgeOffset.x(), -centerEdgeOffset.y());
        }
        else // direction.y() == 0，水平运动
        {
            leftcenterEdgeOffset = QPointF(0, centerEdgeOffset.y());
            RightcenterEdgeOffset = QPointF(0, -centerEdgeOffset.y());
        }
    }
    else // direction.x() == 0，竖直运动
    {
        if (direction.y() > 0) // 向下运动
        {
            leftcenterEdgeOffset = QPointF(centerEdgeOffset.x(), 0.0);
            RightcenterEdgeOffset = QPointF(-centerEdgeOffset.x(), 0.0);
        }
        else if (direction.y() < 0) // 向上运动
        {
            leftcenterEdgeOffset = QPointF(-centerEdgeOffset.x(), 0);
            RightcenterEdgeOffset = QPointF(centerEdgeOffset.x(), 0);
        }
        else // direction.y() == 0，静止状态
        {
            leftcenterEdgeOffset = QPointF(0, 0);
            RightcenterEdgeOffset = QPointF(0, 0);
        }
    }
}

// 计算相对角度
double ToolMover::calculateAngle(const QPointF& point, const QPointF& centerpoint) {
    // 计算起点向量
    QPointF startVec =point- centerpoint;

    double startAngle;

    // 计算起始角度
    if (startVec.x() == 0 && startVec.y() == 0) {
        // 起点与圆心重合，设定一个默认角度
        startAngle = 0; // 或者选择其他合适的值
    } else if (startVec.x() == 0) {
        // 处理竖直线情况
        startAngle = (startVec.y() > 0) ? M_PI / 2 : (3 * M_PI / 2); // 竖直向上或向下
    } else {
        startAngle = std::atan2(startVec.y(), startVec.x()); // 起始角度
    }

    // 确保 startAngle 在 0 到 2π 的范围内
    if (startAngle < 0) {
        startAngle += 2 * M_PI; // 将负值转换为正值
    }

    // 判断特殊角度并进行处理
    if (fabs(startAngle) < 1e-6) {
        startAngle = 0; // 0度
    } else if (fabs(startAngle - M_PI / 2) < 1e-6) {
        startAngle = M_PI / 2; // 90度
    } else if (fabs(startAngle - M_PI) < 1e-6) {
        startAngle = M_PI; // 180度
    } else if (fabs(startAngle - (3 * M_PI / 2)) < 1e-6) {
        startAngle = 3 * M_PI / 2; // 270度
    }

    return startAngle; // 返回结果
}

